-
Notifications
You must be signed in to change notification settings - Fork 156
New issue
Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.
By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.
Already on GitHub? Sign in to your account
Enable parallel planning with PipelinePlanner #450
Conversation
Have a look at #429, which provides parallel planning functionality not only including pipeline planners but also joint interpolation and Cartesian interpolation. |
Cool! I was not aware of this. As I understand it, it is still different from the proposed Parallel Planning solver as you can not customize stopping and solution selection criteria and the MultiPlanner is capable of using multiple MTC solvers at the same time. So I'd see both as alternatives. |
That's true. The MultiPlanner defines a sequence of planning alternatives that are tried one after the other, while parallel planning considers all alternatives in parallel. Would it be possible to apply the same approach as in MultiPlanner to realize your goal? This would allow to include joint-space and Cartesian-space interpolation planners as alternatives as well. By the way, a stopping criterium might be useful for the existing sequential MultiPlanner as well - to continue planning if the criterium (e.g. smoothness) is not yet satisfied. What about the new names |
That's a good idea 👍 I'll do that and use this chance to port the MultiPlanner over to the ros2 branch and rename it |
Codecov ReportAttention:
Additional details and impacted files@@ Coverage Diff @@
## ros2 #450 +/- ##
==========================================
+ Coverage 41.67% 42.71% +1.04%
==========================================
Files 79 80 +1
Lines 7939 7945 +6
==========================================
+ Hits 3308 3393 +85
+ Misses 4631 4552 -79
☔ View full report in Codecov by Sentry. |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Looking good! I have a few minor comments.
I tried your tutorial fork and it works, but the placement pose doesn't seem correct. However, also seems to look like that on main
, so I don't think it's a problem with this branch.
2023-04-13.07-47-18.mp4
core/include/moveit/task_constructor/solvers/pipeline_planner.h
Outdated
Show resolved
Hide resolved
As stated in #451 (comment), I don't see added value in this PR in addition to #451. Before you put more effort into this, we should agree on the general route... |
Yes 👍 I answered in the other thread |
Is this actually planning in parallel? The console output looks suspiciously in series, but maybe that's just the ROS logging framework.
Why are we getting the message
when we have |
It's the ROS logging. Look at the first two lines, PILZ and OMPL logging are interwoven which wouldn't be the case if it is sequential: [mtc_tutorial-1] [INFO] [1681497796.624044521] [moveit.ompl_planning.model_based_planning_context]: Planner configuration 'panda_arm' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed.
[mtc_tutorial-1] [INFO] [1681497796.661303311] [moveit.::planning_interface.planning_pipeline_interfaces]: Stopping criterion met: Terminating planning pipelines that are still active
[mtc_tutorial-1] [INFO] [1681497796.684124377] [moveit.pilz_industrial_motion_planner.trajectory_generator]: Generating LIN trajectory...
[mtc_tutorial-1] [INFO] [1681497796.684154697] [moveit.ompl_planning.model_based_planning_context]: Planner configuration 'panda_arm[RRTConnectkConfigDefault]' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed.
I fixed that, the parallel_planning_solver worked fine but the sampling_solver uses the parallel planning interface with a single pipeline. The previously configured default stopping criteria was met by that planner (which doesn't have any effect since it is a single pipeline anyways). I updated the default, so this doesn't happen anymore |
commit 8e2c560 Author: Sebastian Jahr <[email protected]> Date: Mon Apr 17 09:49:51 2023 +0200 Use no default stopping criteria commit d095f7f Author: Sebastian Jahr <[email protected]> Date: Fri Apr 14 12:20:44 2023 +0200 Refactor to avoid calling .at(0) twice commit 4238dc3 Author: Sebastian Jahr <[email protected]> Date: Fri Apr 14 12:10:45 2023 +0200 Format commit 2c74526 Author: Sebastian Jahr <[email protected]> Date: Fri Apr 14 10:39:26 2023 +0200 Update core/src/solvers/pipeline_planner.cpp commit f06a93c Author: Sebastian Jahr <[email protected]> Date: Fri Apr 14 10:34:02 2023 +0200 Update core/src/solvers/pipeline_planner.cpp Co-authored-by: Sebastian Castro <[email protected]> commit 168cbd5 Author: Sebastian Jahr <[email protected]> Date: Tue Apr 11 11:14:19 2023 +0200 Small clang-tidy fix commit c0ea5cd Author: Sebastian Jahr <[email protected]> Date: Thu Apr 6 10:34:20 2023 +0200 Pass pipeline map by reference commit 9be2ab3 Author: Sebastian Jahr <[email protected]> Date: Fri Mar 31 17:55:58 2023 +0200 Add API to set parallel planning callbacks and deprecate functions commit 132235f Author: Sebastian Jahr <[email protected]> Date: Thu Mar 30 20:18:50 2023 +0200 Cleanup and documentation commit 7dd2743 Author: Sebastian Jahr <[email protected]> Date: Thu Mar 30 17:12:44 2023 +0200 Add callbacks commit f050adb Author: Sebastian Jahr <[email protected]> Date: Thu Mar 30 16:00:25 2023 +0200 Enable configuring multiple pipelines commit b96992c Author: Sebastian Jahr <[email protected]> Date: Thu Mar 30 10:29:47 2023 +0200 Make usable with parallel planning commit 2a7234d Author: Sebastian Jahr <[email protected]> Date: Thu Mar 30 09:47:38 2023 +0200 Re-order plan functions commit 6679c98 Author: Sebastian Jahr <[email protected]> Date: Tue Mar 28 15:28:02 2023 +0200 Make code readable
PipelinePlanner(const rclcpp::Node::SharedPtr& node, const std::string& pipeline_name = "ompl", | ||
const std::string& planner_id = ""); | ||
|
||
[[deprecated("Deprecated: Use new constructor implementations.")]] // clang-format off |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Would it make sense to extend from PipelinePlanner instead of deprecating the old interface?
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
With the current implementation, this constructor would be underdefined because to my understanding it is not possible to infer the default planner name from the planning pipeline pointer because that information is hidden inside the planner plugin
* \param [in] solution_selection_function Callback function to choose the best solution when multiple pipelines are used | ||
*/ | ||
PipelinePlanner(const rclcpp::Node::SharedPtr& node, | ||
const std::unordered_map<std::string, std::string>& pipeline_id_planner_id_map, |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
usually, pipelines have default algorithms configured, right? Shouldn't the planner_id map specified per stage, if at all? The properties would be a great place to specify planner ids
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
I think the planner_id map should be defined because rather than using unconsciously some default algorithm, I think it should be explicitly configured which one is used.
To have an additional property to override this pipeline_id_planner_id_map on the stage level is a good idea. With that it would be possible to use a subset of the defined planner-pipeline pairs. Would this require more work in the stage implementations? If yes, I think this should be follow-up work.
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
I think the last commit achieves what you requested:
f57e659
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
I noticed these warnings when building moveit_task_constructor_demos
with the latest version of MoveIt 2.
#39 1131.7 --- stderr: moveit_task_constructor_demo
#39 1131.7 /opt/underlay_ws/src/moveit_task_constructor/demo/src/fallbacks_move_to.cpp: In lambda function:
#39 1131.7 /opt/underlay_ws/src/moveit_task_constructor/demo/src/fallbacks_move_to.cpp:35:33: warning: ‘void moveit::task_constructor::solvers::PipelinePlanner::setPlannerId(const string&)’ is deprecated: Replaced with setPlannerId(pipeline_name, planner_id) [-Wdeprecated-declarations]
#39 1131.7 35 | pp->setPlannerId("PTP");
#39 1131.7 | ~~~~~~~~~~~~~~~~^~~~~~~
#39 1131.7 In file included from /opt/underlay_ws/src/moveit_task_constructor/demo/src/fallbacks_move_to.cpp:9:
#39 1131.7 /opt/underlay_ws/install/moveit_task_constructor_core/include/moveit/task_constructor/solvers/pipeline_planner.h:91:14: note: declared here
#39 1131.7 91 | void setPlannerId(const std::string& /*planner*/) { /* Do nothing */
#39 1131.7 | ^~~~~~~~~~~~
#39 1131.7 /opt/underlay_ws/src/moveit_task_constructor/demo/src/fallbacks_move_to.cpp: In lambda function:
#39 1131.7 /opt/underlay_ws/src/moveit_task_constructor/demo/src/fallbacks_move_to.cpp:41:33: warning: ‘void moveit::task_constructor::solvers::PipelinePlanner::setPlannerId(const string&)’ is deprecated: Replaced with setPlannerId(pipeline_name, planner_id) [-Wdeprecated-declarations]
#39 1131.7 41 | pp->setPlannerId("RRTConnect");
#39 1131.7 | ~~~~~~~~~~~~~~~~^~~~~~~~~~~~~~
#39 1131.7 In file included from /opt/underlay_ws/src/moveit_task_constructor/demo/src/fallbacks_move_to.cpp:9:
#39 1131.7 /opt/underlay_ws/install/moveit_task_constructor_core/include/moveit/task_constructor/solvers/pipeline_planner.h:91:14: note: declared here
#39 1131.7 91 | void setPlannerId(const std::string& /*planner*/) { /* Do nothing */
#39 1131.7 | ^~~~~~~~~~~~
So I think this PR should also update those accordingly.
Make code readable Re-order plan functions Make usable with parallel planning Enable configuring multiple pipelines Add callbacks Cleanup and documentation Add API to set parallel planning callbacks and deprecate functions Pass pipeline map by reference Small clang-tidy fix Update core/src/solvers/pipeline_planner.cpp Co-authored-by: Sebastian Castro <[email protected]> Update core/src/solvers/pipeline_planner.cpp Format Refactor to avoid calling .at(0) twice Use no default stopping criteria Update fallbacks_move demo
d6c3ef2
to
f57e659
Compare
@rhaschke @henningkayser Do you mind giving this a review? |
The ros2 branch in codev is out-of-date https://app.codecov.io/gh/ros-planning/moveit_task_constructor/tree/ros2 |
Looks like re-triggering the CI job for the ros2 branch fixed it |
* Refactor pipeline planner Make code readable Re-order plan functions Make usable with parallel planning Enable configuring multiple pipelines Add callbacks Cleanup and documentation Add API to set parallel planning callbacks and deprecate functions Pass pipeline map by reference Small clang-tidy fix Update core/src/solvers/pipeline_planner.cpp Co-authored-by: Sebastian Castro <[email protected]> Update core/src/solvers/pipeline_planner.cpp Format Refactor to avoid calling .at(0) twice Use no default stopping criteria Update fallbacks_move demo * Cleanup + address deprecation warnings * Enabling optionally using a property defined pipeline planner map * Address review * Disable humble CI for ros2 branch * Add pipeline planner unittests + some checks * Add short comment
This reverts commit 0e02fca.
This reverts commit 0e02fca.
This reverts commit 0e02fca.
This reverts commit 0e02fca.
This reverts commit 0e02fca.
This reverts commit 0e02fca.
This reverts commit 0e02fca.
This reverts commit 0e02fca.
This reverts commit 0e02fca.
This reverts commit 0e02fca.
This reverts commit 0e02fca.
This reverts commit 0e02fca.
This reverts commit 0e02fca.
This reverts commit 0e02fca.
This reverts commit 0e02fca.
This reverts commit 0e02fca.
This reverts commit 0e02fca.
This reverts commit 0e02fca.
This reverts commit 0e02fca.
This reverts commit 0e02fca.
This reverts commit 0e02fca.
This PR refactors the PipelinePlanner to enable the use of MoveIt's parallel planning API. Usage would look for example like this
You can test the PR by using my fork of the moveit2_tutorials. Build the custom MTC and moveit2_tutorials branch and run in two terminal windows:
ros2 launch moveit2_tutorials mtc_demo.launch.py
ros2 launch moveit2_tutorials pick_place_demo.launch.py